"""
主要控制函数，
即主控系统，用于控制整个系统的运行
各类在这里调用
"""
import base64
import hashlib
import json
import os
import time
import re
import numpy as np
from scipy.stats import tmean
from sympy.codegen.ast import continue_

from src.lqr_controller import LQR_controller
import threading
import copy
# from src.MpcControl.MPC_control import MPCController
from src.straight_controller import Straight_controller
from src.connection.Connect import Connect
from bins.boundary import Boundary
from bins import vehiclelogger
import logging

logger = logging.getLogger(__name__)
kk = re.compile(r'\d+\.?\d+|\d+|-\d+\.?\d+')
start_event = threading.Event()
stop_event = threading.Event()

current_state = vehiclelogger.VehicleStateLogger()


def monitor_car_state(car_state):  # car_state 为字典
    x = car_state["x"]
    y = car_state["y"]
    h = car_state["h"]
    v = car_state["v"]
    str_log = f"x={x} y={y} h={h} v={v}"
    logger.info(str_log)


class MainControl:
    def __init__(self):
        self.connect = Connect()
        self.boundary = Boundary()
        self.lqr = LQR_controller()
        self.tk = Straight_controller()
        self.recode_thread = None
        self.traj_file = 'trajeo.txt'
        self.calibration_file = 'calibration.txt'
        self.current_state = vehiclelogger.SharedVariable()
        self.mode = 0
        self.send = 0

    def init(self) -> bool:
        loadconfig = json.loads(open("./conf/serial_port.json", "r").read())
        self.connect.ser = loadconfig["ser"]
        self.connect.serPC = loadconfig["serPC"]
        if self.connect.connect():
            logger.info("Connect successfully")
        else:
            return False
        self.boundary.load_boundary()
        return True

    def processing_input(self):
        order = self.connect.getPCinput()
        if self.send == 0:
            if order == "track":
                self.mode = 2
                self.lqr.lqr_mode = 1
                logger.info("Track mode")
            elif order == "normal":
                if self.mode == 2:
                    self.mode = 1
                    time.sleep(0.5)
                # TODO: tk需要另外处理
                self.tk.thr = 0
                self.tk.steer = 0
                self.tk.brk = 0
                self.connect.car_control(0, 0, 0)
                logger.info("Normal mode")
            elif order == "sdstart":
                self.send = 1
                logger.info("Start receiving data")
                self.connect.serPC.flushOutput()
                jarFile = open("trajeo.txt", "wb+")
                jarFile.close()
            elif order == "updata":
                try:
                    preFile = np.loadtxt('trajeo.txt')
                except Exception as e:
                    logger.error(f"Error in reading file: {e}")
                    return
                # try:
                x = preFile[:, 0]
                y = preFile[:, 1]
                h = preFile[:, 2]
                v = preFile[:, 3]
                t = preFile[:, 4]
                current_state.load_track(x = list(x), y = list(y), h = list(h), v = list(v), t = list(t))
                self.lqr.update_trajectory(x, y, h, v, t)
                self.send = 1
                if self.connect.sendtoPC("finish\n"):
                    time.sleep(0.2)
                if self.connect.sendtoPC("finish\n"):
                    logger.info("Finish in updata trajectory")
                else:
                    logger.info("Error in send finish command to PC")
                self.send = 0
                # except:
                #     logger.error("Error in updating trajectory")
            elif order[:5] == 'Ready':
                PID = kk.findall(order)
                self.lqr.kp = float(PID[0])
                self.lqr.ki = float(PID[1])
                self.lqr.kd = float(PID[2])
                self.tk.kSteer = float(PID[3])
                self.boundary.update_pid(PID)
                logger.info('finsh PID ' + str(PID))

            elif order[:6] == 'Linear':
                self.mode = 3
                r = r'[-0-9.0-9]+'
                loca = re.findall(r, order)
                self.tk.Linearx2 = float(loca[0])
                self.tk.Lineary2 = float(loca[1])
                self.tk.goal_v = float(loca[2])
                self.tk.model = float(loca[3])
                logger.info(
                    f"have receive the data:x2 = {self.tk.Linearx2},y2 = {self.tk.Lineary2},goal_v = {self.tk.goal_v}")
                # tk.xylinear(0, 0, loca[0], loca[1])               # 录入起点终点位置
                self.tk.thr = 0.0
                self.tk.steer = 0.0
                self.tk.brake = 0.2
                self.connect.car_control(self.tk.thr, self.tk.steer, self.tk.brake)
                self.tk.i = 0
            elif order[:4] == 'Edge':
                EdGe = kk.findall(order)
                self.boundary.update_boundary(EdGe)
                logger.info(f"finish update boundary {EdGe}")
            elif order == 'reseti':
                self.tk.i = 0
                logger.info("reset i")
            elif order == 'restart':
                os.system('sudo reboot')
                logger.info("restart")
            elif order == 'poweroff':
                os.system('sudo poweroff')
                logger.info("poweroff")
        else:
            if order[:4] == 'wait':
                self.send = 0
                logger.info("wait")
            elif order[:6] == 'sdpart':
                logger.info("sdpart decode")
                data = base64.b64decode(order[6:])
                jarFile = open(self.traj_file, "ab+")
                jarFile.write(data)
                jarFile.close()
                logger.info("save")
                self.connect.serPC.flushOutput()
            elif order[:6] == 'sdfnsh':
                logger.info("sdfinish")
                self.connect.serPC.flushOutput()
                data = base64.b64decode(order[6:])
                jarFile = open(self.traj_file, "ab+")
                jarFile.write(data)
                jarFile.close()
                sFile = open(self.traj_file, "rb").read()
                md5 = hashlib.md5(sFile).hexdigest()
                print('finishsave' + md5)
                self.connect.sendtoPC('MD5rec:' + md5 + "\n")
                self.send = 0

        if order == 'emstop':
            if self.mode == 2:
                self.mode = 1
                time.sleep(0.5)
            logger.info("emstop")
            self.connect.car_control(0, 0.7, 0)

    def process_PCorder(self):
        while True:
            try:
                self.processing_input()
            except Exception as e:
                logger.error(f"Error in processing input: {e}")

    def process_Carstate(self):
        while True:
            try:
                self.connect.get_car_state(self.send)
                time.sleep(0.05)
            except Exception as e:
                time.sleep(0.1)
                logger.error(f"Error in processing car state:{e}")

    def recode_traj(self):
        while True:
            i = 0
            f = None
            f_calibration = None
            current_state = copy.deepcopy(self.current_state.get())
            try:
                while current_state["m"] == 0:  # 遥控器录制模式
                    current_state = self.current_state.get().copy()
                    if i == 0:
                        logger.info("Begin recording trajectory, all data is saved in trajeo.txt")
                        f = open(self.traj_file, 'w', encoding='utf-8')                     # trajeo
                        f_calibration = open(self.calibration_file, 'w', encoding='utf-8')  # calibration
                        i += 1
                    s = (str(current_state["x"]) + ' ' + str(current_state["y"]) + ' '
                         + str(current_state["h"]) + ' ' + str(current_state["v"]) + ' '
                         + str(time.time())+'\n')
                    s_calibration = (str(current_state["v"]) + ' ' + str(current_state["th"]) + ' '
                                     + str(current_state["br"]) + str(time.time())+'\n')
                    f.write(s)
                    f_calibration.write(s_calibration)
                    time.sleep(0.5)
                    i += 1
            except:
                logger.error("Error in recording trajectory")
                time.sleep(0.1)
            if f is not None:
                logger.info("Finish recording trajectory, all data is saved in trajeo.txt, all length is " + str(i))
                f.close()
                f_calibration.close()

    def car_state_log(self, car_x, car_y, car_h, car_v, car_t, car_thr, car_brk, target_list):
        try:
            # folder_name = "Car_state_file"
            # folder_path = f"./{folder_name}"
            # if not os.path.exists(folder_path):
            #     os.makedirs(folder_path)
            # timestamp_str = time.strftime("%Y-%m-%d_%H_%M_%S")
            # file_path = os.path.join(folder_path, f"target_list_{timestamp_str}.txt")
            if self.mode == 2:
                # target_list = open(file_path, "w", encoding="UTF-8")
                target_list.write(str(car_x) + ' ' + str(car_y) + ' ' + str(car_h) + ' ' + str(car_v) + ' ' + str(car_t) + ' ' + str(car_thr) + ' ' + str(car_brk) + '\n')
                # target_list.flush()
                # target_list.close()
            else:
                pass
            # logger.info("target list loaded")
        except:
            logger.info("car_state_log loaded fail")

    def run(self):
        # 上位机指令接收线程
        readData1 = threading.Thread(target=self.process_PCorder)
        readData1.setDaemon(True)
        readData1.start()
        # 车辆状态接收线程
        readState2 = threading.Thread(target=self.process_Carstate)
        readState2.setDaemon(True)
        readState2.start()
        # 轨迹记录线程
        recodeData3 = threading.Thread(target=self.recode_traj)
        recodeData3.setDaemon(True)
        recodeData3.start()
        # 控制线程
        self.control()

    def control(self):
        folder_name = "Car_state_file"
        folder_path = f"./{folder_name}"
        if not os.path.exists(folder_path):
            os.makedirs(folder_path)
        timestamp_str = time.strftime("%Y-%m-%d_%H_%M_%S")
        file_path = os.path.join(folder_path, f"target_list_{timestamp_str}.txt")
        target_list = open(file_path, "w", encoding="UTF-8")

        while True:
            new_state = None
            current_state.record_timestamp()
            now = time.time()
            try:
                state = self.current_state.get()
                repeat = bool(new_state == state)
                new_state = state
                cur_time = time.time()
                # print(now - cur_time)
                now = cur_time
                if self.mode == 2:
                    self.lqr.update_ego_state(new_state["x"], new_state["y"], new_state["h"], new_state["v"], cur_time, repeat)  # 更新自车状态
                    self.car_state_log(new_state["x"], new_state["y"], new_state["h"], new_state["v"], cur_time, new_state["th"], new_state["br"], target_list)
                    monitor_car_state(new_state)  # 监控车状态
                    lqr_mode = self.lqr.track()
                    logger.info(f"lqr_mode = {lqr_mode}, 0 for not_lqr, 1 for lqr")
                    if lqr_mode == 0 and float(new_state["v"]) <= 0.01:
                        lqr_ = "main_control break 0.95"
                        logger.info(lqr_)
                        self.mode = 1
                        self.connect.car_control(0, 0.95, 0)
                    else:
                        self.lqr.control_log()
                        self.connect.car_control(self.lqr.thr, self.lqr.brake, self.lqr.steer)
                elif self.mode == 3:
                    self.tk.xylinear(new_state["x"], new_state["y"], new_state["h"], new_state["v"])
                    if self.tk.i == 0:
                        self.tk.Linearx1 = new_state["x"]
                        self.tk.Lineary1 = new_state["y"]
                        self.tk.xylinear(self.tk.Linearx1, self.tk.Lineary1, self.tk.Linearx2,
                                         self.tk.Lineary2)  # 录入起点终点位置
                    self.tk.linear()
                    self.connect.car_control(self.tk.thr, self.tk.steer, self.tk.brake)
                else:
                    time.sleep(0.05)
            except Exception as err:  # 异常处理
                time.sleep(0.1)
                logger.error(err)
